package edu.vt.ece.rt.benchmark.labyrinth;

import java.util.ArrayList;
import java.util.LinkedList;
import java.util.Queue;

public class Router {

	long xCost;
	long yCost;
	long zCost;
	long bendCost;

	Router (long xCost, long yCost, long zCost, long bendCost) {
		this.xCost = xCost;
		this.yCost = yCost;
		this.zCost = zCost;
		this.bendCost = bendCost;
	}

	public static long     MOMENTUM_ZERO = 0;
	public static long 	    MOMENTUM_POSX = 1;
	public static long 	    MOMENTUM_POSY = 2;
	public static long     	    MOMENTUM_POSZ = 3;
	public static long 	    MOMENTUM_NEGX = 4;
	public static long  	    MOMENTUM_NEGY = 5;
	public static long 	    MOMENTUM_NEGZ = 6;
	public static Point MOVE_POSX = new Point( 1,  0,  0,  0, MOMENTUM_POSX);
	public static Point MOVE_POSY = new Point(  0,  1,  0,  0, MOMENTUM_POSY);
	public static Point MOVE_POSZ = new Point(  0,  0,  1,  0, MOMENTUM_POSZ);
	public static Point MOVE_NEGX = new Point( -1,  0,  0,  0, MOMENTUM_NEGX);
	public static Point MOVE_NEGY = new Point(  0, -1,  0,  0, MOMENTUM_NEGY);
	public static Point MOVE_NEGZ = new Point(  0,  0, -1,  0, MOMENTUM_NEGZ);



	/* =============================================================================
	 * PexpandToNeighbor
	 * =============================================================================
	 */
	static void
	PexpandToNeighbor (Grid myGridPtr,
			long x, long y, long z, long value, Queue<Coordinate> queuePtr)
	{
		if (Grid.grid_isPointValid(myGridPtr, x, y, z)) {
			Coordinate neighborGridPointPtr = Grid.grid_getPoint(myGridPtr, x, y, z);
			long neighborValue = neighborGridPointPtr.value;
			if (neighborValue == Grid.GRID_POINT_EMPTY) {
				neighborGridPointPtr.value = value;
				queuePtr.add(neighborGridPointPtr);
			} else if (neighborValue != Grid.GRID_POINT_FULL) {
				/* We have expanded here before... is this new path better? */
				if (value < neighborValue) {
					neighborGridPointPtr.value = value;
					queuePtr.add(neighborGridPointPtr);
				}
			}
		}
	}


	/* =============================================================================
	 * PdoExpansion
	 * =============================================================================
	 */
	static boolean
	PdoExpansion (Router routerPtr, Grid myGridPtr, Queue<Coordinate> queuePtr,
			Coordinate srcPtr, Coordinate dstPtr)
	{
		long xCost = routerPtr.xCost;
		long yCost = routerPtr.yCost;
		long zCost = routerPtr.zCost;

		/*
		 * Potential Optimization: Make 'src' the one closest to edge.
		 * This will likely decrease the area of the emitted wave.
		 */

		queuePtr.clear();
		Coordinate srcGridPointPtr =
				Grid.grid_getPoint(myGridPtr, srcPtr.x, srcPtr.y, srcPtr.z);
		queuePtr.add(srcGridPointPtr);
		Grid.grid_setPoint(myGridPtr, srcPtr.x, srcPtr.y, srcPtr.z, 0);
		Grid.grid_setPoint(myGridPtr, dstPtr.x, dstPtr.y, dstPtr.z, Grid.GRID_POINT_EMPTY);
		Coordinate dstGridPointPtr =
				Grid.grid_getPoint(myGridPtr, dstPtr.x, dstPtr.y, dstPtr.z);
		boolean isPathFound = false;

		while (queuePtr.size() != 0) {

			Coordinate gridPointPtr = queuePtr.poll();
			if (Coordinate.coordinate_isEqual(srcGridPointPtr, dstGridPointPtr)) {
				isPathFound = true;
				break;
			}

			long x = gridPointPtr.x;
			long y = gridPointPtr.y;
			long z = gridPointPtr.z;
			long value = gridPointPtr.value;

			/*
			 * Check 6 neighbors
			 *
			 */
			 PexpandToNeighbor(myGridPtr, x+1, y,   z,   (value + xCost), queuePtr);
			 PexpandToNeighbor(myGridPtr, x-1, y,   z,   (value + xCost), queuePtr);
			 PexpandToNeighbor(myGridPtr, x,   y+1, z,   (value + yCost), queuePtr);
			 PexpandToNeighbor(myGridPtr, x,   y-1, z,   (value + yCost), queuePtr);
			 PexpandToNeighbor(myGridPtr, x,   y,   z+1, (value + zCost), queuePtr);
			 PexpandToNeighbor(myGridPtr, x,   y,   z-1, (value + zCost), queuePtr);

		} /* iterate over work queue */

		return isPathFound;
	}

	/* =============================================================================
	 * traceToNeighbor
	 * =============================================================================
	 */
	static void
	traceToNeighbor (Grid myGridPtr,
			Point currPtr,
			Point movePtr,
			boolean useMomentum,
			long bendCost,
			Point nextPtr)
	{
		long x = currPtr.x + movePtr.x;
		long y = currPtr.y + movePtr.y;
		long z = currPtr.z + movePtr.z;

		if (Grid.grid_isPointValid(myGridPtr, x, y, z) &&
				!Grid.grid_isPointEmpty(myGridPtr, x, y, z) &&
				!Grid.grid_isPointFull(myGridPtr, x, y, z))
		{
			long value = Grid.grid_getPoint(myGridPtr, x, y, z).value;
			long b = 0;
			if (useMomentum && (currPtr.momentum != movePtr.momentum)) {
				b = bendCost;
			}
			if ((value + b) <= nextPtr.value) { /* '=' favors neighbors over current */
					nextPtr.x = x;
			nextPtr.y = y;
			nextPtr.z = z;
			nextPtr.value = value;
			nextPtr.momentum = movePtr.momentum;
			}
		}
	}


	/* =============================================================================
	 * PdoTraceback
	 * =============================================================================
	 */
	static ArrayList<Coordinate>
	PdoTraceback (Grid gridPtr, Grid myGridPtr,
			Coordinate dstPtr, long bendCost)
			{
		ArrayList<Coordinate> pointVectorPtr = new ArrayList<Coordinate>();

		Point next = new Point();
		next.x = dstPtr.x;
		next.y = dstPtr.y;
		next.z = dstPtr.z;
		next.value = Grid.grid_getPoint(myGridPtr, next.x, next.y, next.z).value;
		next.momentum = MOMENTUM_ZERO;

		while (true) {

			Coordinate gridPointPtr = Grid.grid_getPoint(gridPtr, next.x, next.y, next.z);
			pointVectorPtr.add(gridPointPtr);
			Grid.grid_setPoint(myGridPtr, next.x, next.y, next.z, Grid.GRID_POINT_FULL);

			/* Check if we are done */
			if (next.value == 0) {
				break;
			}
			Point curr = next;

			/*
			 * Check 6 neighbors
			 *
			 */
			traceToNeighbor(myGridPtr, curr, MOVE_POSX, true, bendCost, next);
			traceToNeighbor(myGridPtr, curr, MOVE_POSY, true, bendCost, next);
			traceToNeighbor(myGridPtr, curr, MOVE_POSZ, true, bendCost, next);
			traceToNeighbor(myGridPtr, curr, MOVE_NEGX, true, bendCost, next);
			traceToNeighbor(myGridPtr, curr, MOVE_NEGY, true, bendCost, next);
			traceToNeighbor(myGridPtr, curr, MOVE_NEGZ, true, bendCost, next);

			/*
			 * Because of bend costs, none of the neighbors may appear to be closer.
			 * In this case, pick a neighbor while ignoring momentum.
			 */
			if ((curr.x == next.x) &&
					(curr.y == next.y) &&
					(curr.z == next.z))
			{
				next.value = curr.value;
				traceToNeighbor(myGridPtr, curr, MOVE_POSX, false, bendCost, next);
				traceToNeighbor(myGridPtr, curr, MOVE_POSY, false, bendCost, next);
				traceToNeighbor(myGridPtr, curr, MOVE_POSZ, false, bendCost, next);
				traceToNeighbor(myGridPtr, curr, MOVE_NEGX, false, bendCost, next);
				traceToNeighbor(myGridPtr, curr, MOVE_NEGY, false, bendCost, next);
				traceToNeighbor(myGridPtr, curr, MOVE_NEGZ, false, bendCost, next);

				if ((curr.x == next.x) &&
						(curr.y == next.y) &&
						(curr.z == next.z))
				{
					return null; /* cannot find path */
				}
			}
		}

		return pointVectorPtr;
			}

	/* =============================================================================
	 * router_solve
	 * =============================================================================
	 */
	static void
	router_solve ( Router routerPtr, Maze mazePtr, ArrayList<ArrayList<Coordinate>> pathVectorListPtr)
	{

		ArrayList<Coordinate> myPathVectorPtr = new ArrayList<Coordinate>();

		Queue<Pair> workQueuePtr = mazePtr.workQueuePtr;
		Grid gridPtr = mazePtr.gridPtr;
		Grid myGridPtr = new Grid(gridPtr);
		long bendCost = routerPtr.bendCost;
		Queue<Coordinate> myExpansionQueuePtr = new LinkedList<Coordinate>();

		/*
		 * Iterate over work list to route each path. This involves an
		 * 'expansion' and 'traceback' phase for each source/destination pair.
		 */
		
		while (true) {

			Pair coordinatePairPtr;
			if (workQueuePtr.size() == 0) {
				coordinatePairPtr = null;
			} else {
				coordinatePairPtr = (Pair)workQueuePtr.poll();
			}

			if (coordinatePairPtr == null) {
				break;
			}

			Coordinate srcPtr = coordinatePairPtr.firstPtr;
			Coordinate dstPtr = coordinatePairPtr.secondPtr;

			boolean success = false;
			ArrayList<Coordinate> pointVectorPtr = null;

			if (PdoExpansion(routerPtr, myGridPtr, myExpansionQueuePtr,
					srcPtr, dstPtr)) {
				pointVectorPtr = PdoTraceback(gridPtr, myGridPtr, dstPtr, bendCost);
				if (pointVectorPtr != null) {
					Grid.grid_addPath(gridPtr, pointVectorPtr);
					success = true;
				}
			}

			pathVectorListPtr.add(myPathVectorPtr);
		}

	}

}
